#include "ctrl_basic.h"
#include "DCMI.h"
#include "ImrealNew.h"


_xy_St _xy_t;
Info_Car_St Info_Car_t;
Car_Flag_St Car_Flag_t;
extern u8 Image_Bin[CAMERA_Fit_H][CAMERA_Fit_W];

void Para_Init(void)
{
	Info_Car_t.Top_XY.x=0;
	Info_Car_t.Top_XY.y=CAMERA_Fit_H-1;
	Info_Car_t.Bottom_XY.x=0;
	Info_Car_t.Bottom_XY.y=0;
	
	Info_Car_t.Left_XY.x=CAMERA_Fit_W-1;
	Info_Car_t.Left_XY.y=0;
	Info_Car_t.Right_XY.x=0;
	Info_Car_t.Right_XY.y=0;
	
	Info_Car_t.Rotate_Off=0;
	
	Car_Flag_t.En_Yaw=No;
	Car_Flag_t.Have_Top_Car=No;
	Car_Flag_t.Have_Bottom_Car=No;
	Car_Flag_t.Reach_Edge_Flag=No;
	Car_Flag_t.Have_Car_Flag=No;
	
}


void Seach_Car_XY()
{
//	_xy_St Right_Edge[50];
	static u8 Last_Mid_x=0;
	static u8 Last_Mid_y=0;
	u8 Gap=3;
	u8 line=0;
	u8 Find_Ok=0;
	u8 CarLeft_X=0,CarRight_X=0;
	u8 Last_Left_X=0,Last_Right_X=0;//��һ������x��
	YesNo_t Have_Dot_Line=No;//�����е�
	YesNo_t Have_Dot_Last_Line=No;//��һ���е�
	YesNo_t Mid_Find_Black=No;//
	u8 Find_Black=0;
	Para_Init();
	for(line=CAMERA_Fit_H/2;line>0;line-=Gap)//�м俪ʼ��
	{
		CarLeft_X=0;
		CarRight_X=0;
		
		if(Find_Ok==0)
			scan_One_Line(line,&CarLeft_X,&CarRight_X);//���ر��к������ҵ�����
		else 
			edge_Track_Line((CarLeft_X+CarRight_X)/2,line,&CarLeft_X,&CarRight_X);
		
		if(CarLeft_X==0&&CarRight_X==0)//�ж����޵�
			Have_Dot_Line=No;
		else
		{
			Car_Flag_t.Have_Top_Car=Yes;
			Have_Dot_Line=Yes;
			if(Find_Black==0)
				Find_Black=1;
		}
		
		if(Have_Dot_Line==Yes&&Have_Dot_Last_Line==Yes&&ABS(CarLeft_X-Last_Left_X)<6)//�жϵ����Ч��
		{
			
			if(line<Info_Car_t.Top_XY.y)
			{
				Info_Car_t.Top_XY.y=line;
				Info_Car_t.Top_XY.x=(Last_Left_X+Last_Right_X)/2;
			}
			if(line>Info_Car_t.Bottom_XY.y)
			{
				Info_Car_t.Bottom_XY.y=line;
				Info_Car_t.Bottom_XY.x=(Last_Left_X+Last_Right_X)/2;
			}
			if(Last_Left_X<Info_Car_t.Left_XY.x)
			{
				Info_Car_t.Left_XY.x=Last_Left_X;
				Info_Car_t.Left_XY.y=line;
			}
			if(Last_Right_X>Info_Car_t.Right_XY.x)
			{
				Info_Car_t.Right_XY.x=Last_Right_X;
				Info_Car_t.Right_XY.y=line;
			}
		}	
		
		Last_Left_X=CarLeft_X;
		Last_Right_X=CarRight_X;
		Have_Dot_Last_Line=Have_Dot_Line;
		
		if(line==CAMERA_Fit_H/2&&ABS(CarLeft_X-CarRight_X)>2&&ABS(CarLeft_X-CarRight_X)<60)//�м��е�
			Mid_Find_Black=Yes;
		if(line-Gap<=0)//��ֹԽ��
			Gap=1;
		if(Find_Black==1)
		{
			line+=Gap;//��3��
			Gap=1;//һ��һ����
			Find_Black=2;
		}
		
	}
	
	
	if(Mid_Find_Black==Yes||Car_Flag_t.Have_Top_Car==No)
	{
		Find_Black=0;
		for(line=CAMERA_Fit_H/2;line<CAMERA_Fit_H-1;line+=Gap)
		{
			CarLeft_X=0;
			CarRight_X=0;
			if(Find_Ok==0)
				scan_One_Line(line,&CarLeft_X,&CarRight_X);//���ر��к������ҵ�����
			else 
				edge_Track_Line((CarLeft_X+CarRight_X)/2,line,&CarLeft_X,&CarRight_X);
			
			if(CarLeft_X==0&&CarRight_X==0)//�ж����޵�
				Have_Dot_Line=No;
			else 
			{
				Car_Flag_t.Have_Bottom_Car=Yes;
				Have_Dot_Line=Yes;
				if(Find_Black==0)
					Find_Black=1;
			}
		
			if(Have_Dot_Line==Yes&&Have_Dot_Last_Line==Yes&&ABS(CarLeft_X-Last_Left_X)<6)//�жϵ����Ч��
			{
				if(line<Info_Car_t.Top_XY.y)
				{
					Info_Car_t.Top_XY.y=line;
					Info_Car_t.Top_XY.x=(Last_Left_X+Last_Right_X)/2;
				}
				if(line>Info_Car_t.Bottom_XY.y)
				{
					Info_Car_t.Bottom_XY.y=line;
					Info_Car_t.Bottom_XY.x=(Last_Left_X+Last_Right_X)/2;
				}
				if(Last_Left_X<Info_Car_t.Left_XY.x)
				{
					Info_Car_t.Left_XY.x=Last_Left_X;
					Info_Car_t.Left_XY.y=line;
				}
				if(Last_Right_X>Info_Car_t.Right_XY.x)
				{
					Info_Car_t.Right_XY.x=Last_Right_X;
					Info_Car_t.Right_XY.y=line;
				}
			}				
			Last_Left_X=CarLeft_X;
			Last_Right_X=CarRight_X;
			Have_Dot_Last_Line=Have_Dot_Line;


			if(line+Gap>=CAMERA_Fit_H-1)//��ֹԽ��
				Gap=1;
			if(Find_Black==1)
			{
				line-=Gap;//��3��
				Gap=1;//һ��һ����
				Find_Black=2;
			}
			
		}
	}
	if(Info_Car_t.Top_XY.y<5||Info_Car_t.Bottom_XY.y>CAMERA_Fit_H-5||Info_Car_t.Left_XY.x<5
			||Info_Car_t.Right_XY.x>CAMERA_Fit_W-5)//�Ӵ�����Ե��
		Car_Flag_t.Reach_Edge_Flag=Yes;
	
	if(Car_Flag_t.Have_Bottom_Car==No&&Car_Flag_t.Have_Top_Car==No)//���¶�û��
			Car_Flag_t.Have_Car_Flag=No;
	else 
			Car_Flag_t.Have_Car_Flag=Yes;	
	
	if(Car_Flag_t.Have_Car_Flag==Yes)
	{
		Last_Mid_x=Info_Car_t.Mid_xy.x;
		Last_Mid_y=Info_Car_t.Mid_xy.y;
		Info_Car_t.Mid_xy.x=(Info_Car_t.Left_XY.x+Info_Car_t.Right_XY.x)/2;
		Info_Car_t.Mid_xy.y=(Info_Car_t.Top_XY.y+Info_Car_t.Bottom_XY.y)/2;
	}
	else 
	{
		Info_Car_t.Mid_xy.x=Last_Mid_x;
		Info_Car_t.Mid_xy.y=Last_Mid_y;
	}
	if(Car_Flag_t.Reach_Edge_Flag==No)
	{
			if(Sque_Dif(Info_Car_t.Top_XY.x,Info_Car_t.Top_XY.y,Info_Car_t.Right_XY.x,Info_Car_t.Right_XY.y)>
			Sque_Dif(Info_Car_t.Bottom_XY.x,Info_Car_t.Bottom_XY.y,Info_Car_t.Right_XY.x,Info_Car_t.Right_XY.y))//�����������
		
			fine_Offest_Rotate(Info_Car_t.Top_XY.y,Info_Car_t.Right_XY.y);
		else 
			fine_Offest_Rotate(Info_Car_t.Bottom_XY.y,Info_Car_t.Right_XY.y);
	}
}	
void	fine_Offest_Rotate(u8 begin,u8 end)
{
		#define Num ((CAMERA_Fit_H/2)+5)
		u8 STA_Pos;
		u8 END_Pos;
		u8 i;
		u8 Buff_Begin[Num]={0};
		u8 Buff_End[Num]={0};
		u8 Find_OK=0;
		s16 sum=0;

		if(Car_Flag_t.En_Yaw==No)
		{
			Info_Car_t.Rotate_Off=0;
			return;
		}
		STA_Pos=begin;
		END_Pos=end;
		for(i=0;i<(END_Pos-STA_Pos);i++)
		{
			Buff_Begin[i]=CAMERA_Fit_W-1;//�г�ʼ����λ��Ϊ���һ��
			if(Find_OK==0)//���ȫ��ɨ��
			{
					Find_OK=scan_One_Line(i+STA_Pos,&Buff_Begin[i],&Buff_End[i]);
			}
			else if(Find_OK==1)//׷��ɨ��
			{
				Find_OK=edge_Track_Line((Buff_Begin[i-1]+Buff_End[i-1])/2,STA_Pos+i,&Buff_Begin[i],&Buff_End[i]);
			}
		}
		for(i=0;i<(END_Pos-STA_Pos);i++)
		{
			sum+=(Buff_End[END_Pos-STA_Pos-1]-Buff_End[i]);
		}
		Info_Car_t.Rotate_Off=sum;	
}
	
/*YesNo_t Vaild_Dot(u8 x,u8 y)
{
	
}*/
/*******************************************************************************
  * @��������   Scan_One_Line(u8 HEIGHT,u8* Line_Begin,u8* Line_End,u8 i,Position_Enum Pos)
  * @����˵��   ɨ��ȫ��
  * @�������   LINE:��
								Line_Begin:������ʼ����洢
								Line_End:���߽�������洢
								i:����λ��
								Pos:��λ
	* @���ز���   ��һ���Ƿ�ʼ����׷�٣�1��ʼ
  * @������ע		��������޵�,Line_End[i]��	Line_Begin[i]�ᱻ��0
*******************************************************************************/
#define LEN 3 //
u8 scan_One_Line(u8 LINE,u8* X_Begin,u8* X_End)
{

		u8 coloum;
		u8 length=0;
		for(coloum=1;coloum<CAMERA_Fit_W-1;coloum++)
		{
			if(Image_Bin[LINE][coloum]==Dot_Black)
				length++;
			
			if(coloum>0 && Image_Bin[LINE][coloum]-Image_Bin[LINE][coloum-1]==-255)
			{ 
				if((coloum>CAMERA_Fit_W-3) || (coloum<=(CAMERA_Fit_W-3) && Image_Bin[LINE][coloum+1]==Dot_Black
				&& Image_Bin[LINE][coloum+2]==Dot_Black))
				{
					*X_Begin=coloum;
				}
			}
			else if(coloum>0 && Image_Bin[LINE][coloum]-Image_Bin[LINE][coloum-1]==255)
			{
				if((coloum>CAMERA_Fit_W-3) || (coloum<=(CAMERA_Fit_W-3)&&Image_Bin[LINE][coloum+1]==Dot_White
				&&Image_Bin[LINE][coloum+2]==Dot_White && length>LEN))//�������������漸���㶼�ǰ׵ģ���֤��������������
				{
					*X_End=coloum;
				}
				
			}
		}
		
		if(*X_End==0&&length>(CAMERA_Fit_W-*X_Begin-10))
			*X_End=CAMERA_Fit_W-1;
	/*	if(ABS(ABS(*X_End-*X_Begin)-length)>6)
		{
			*X_Begin=0;
			*X_End=0;
		}*/
		
		if(*X_Begin>CAMERA_Fit_W-15&& *X_End==0)//�����һ�е�ĩβ��ͬʱ�����յ�Ϊ0
			*X_End=CAMERA_Fit_W-1;
		if(*X_End==0)
				return 0;//��һ��ȫ��ɨ��
		else if(ABS(*X_End-*X_Begin)<20)
				return 1;
		else return 0;

}

/*******************************************************************************
  * @��������   Edge_Track_Line(u8 x,u8 HEIGHT,u8* Line_Begin,u8* Line_End,u8 i,Position_Enum Pos)
  * @����˵��   ����׷�٣��У�
  * @�������   x:��һ�к����е�����
	* @���ز���   ���α���׷���Ƿ�ɹ�
*******************************************************************************/
u8 edge_Track_Line(u8 x,u8 LINE,u8* X_Begin,u8* X_End)
{

	u8 begin=0;
	u8 End=0;
	u8 coloum;
	u8 length=0;
	
	if(x>CAMERA_Fit_W-20)//���Ƴ������Χ
		End=CAMERA_Fit_W-1;
	else 
		End=x+15;
	if(x<16)//���Ƴ�����С��Χ
		begin=1;
	else 
		begin=x-15;
	
	//ɨ�豾�У��趨��ʼ�ͽ�β����
	for(coloum=begin;coloum<End;coloum++)
	{
		if(Image_Bin[LINE][coloum]==Dot_Black)//��¼���к��ߵĳ���
			length++;
		
			if(coloum>0 && Image_Bin[LINE][coloum]-Image_Bin[LINE][coloum-1]==-255)//�������һ���ɸߵ��͵����䣨�׵��ڣ�
			{ 
				if((coloum>CAMERA_Fit_W-3) || (coloum<=(CAMERA_Fit_W-3) && Image_Bin[LINE][coloum+1]==Dot_Black
				&& Image_Bin[LINE][coloum+2]==Dot_Black))        //�������������漸���㶼�Ǻڵģ���֤��������������
				{
					*X_Begin=coloum;
				}
			}
			else if(coloum>0 && Image_Bin[LINE][coloum]-Image_Bin[LINE][coloum-1]==255)//�������һ���ɸߵ��͵�����
			{
				if((coloum>CAMERA_Fit_W-3) || (coloum<=(CAMERA_Fit_W-3)&&Image_Bin[LINE][coloum+1]==Dot_White
				&&Image_Bin[LINE][coloum+2]==Dot_White && length>LEN))//�������������漸���㶼�ǰ׵ģ���֤��������������
				{
					*X_End=coloum;
				}
				
			}
	}
	
	/*if(ABS(ABS(*X_End-*X_Begin)-length)>6)
	{
		*X_Begin=0;
		*X_End=0;
	}*/
	if(*X_End==0&&length>(CAMERA_Fit_W-*X_Begin-10))
			*X_End=CAMERA_Fit_W-1;
	if(*X_Begin==0 && *X_End==0)//
	{
		return scan_One_Line(LINE,X_Begin,X_End);
	}
	else if(ABS(*X_End-*X_Begin)<20)
				return 1;
	else 	
		return 0;
}

uint16 Sque_Dif(uint16_t x1,uint16_t y1,uint16_t x2,uint16_t y2)
{
	return ABS((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
}

